package com.hitqz.robot.dispatch.netty.util;

import com.google.common.collect.Lists;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotMapPosDto;
import org.locationtech.jts.geom.*;
import org.locationtech.jts.geom.prep.PreparedGeometry;
import org.locationtech.jts.geom.prep.PreparedGeometryFactory;

import java.util.LinkedList;
import java.util.List;

/**
 * @author xupkun
 * @date 2024/9/4
 */
public class JtsUtil {


    private static final GeometryFactory geometryFactory = new GeometryFactory();

    /**
     * 扩张 车的长度 1米距离
     * @param cx
     * @param cy
     * @param length
     * @param width
     * @return
     */
    public static Polygon getRectangleVertices(double cx, double cy, double length, double width) {
        if (length <= 0 || width <= 0) {
            throw new IllegalArgumentException("Length and width must be positive.");
        }

        double halfLength = length / 2.0;
        double halfWidth = width / 2.0;

        Coordinate[] coordinates = new Coordinate[5];
        coordinates[0] = new Coordinate(cx - halfLength, cy + halfWidth, 0);
        coordinates[1] = new Coordinate(cx + halfLength, cy + halfWidth, 0);
        coordinates[2] = new Coordinate(cx + halfLength, cy - halfWidth, 0);
        coordinates[3] = new Coordinate(cx - halfLength, cy - halfWidth, 0);
        coordinates[4] = new Coordinate(cx - halfLength, cy + halfWidth,0);

        try {
            //return geometryFactory.createPolygon(coordinates);
            LinearRing shell = geometryFactory.createLinearRing(coordinates);
            return geometryFactory.createPolygon(shell, null);
        } catch (Exception e) {
            // 更详细的异常处理
            System.err.println("Error creating polygon: " + e.getMessage());
            throw new RuntimeException("Failed to create polygon", e);
        }
    }

    public static Polygon createPolygon(double x, double y, double x1, double y1, double x2, double y2, double x3, double y3) {
        Coordinate[] coordinates = new Coordinate[5];
        coordinates[0] = new Coordinate(x,y, 0);
        coordinates[1] = new Coordinate(x1, y1, 0);
        coordinates[2] = new Coordinate(x2, y2, 0);
        coordinates[3] = new Coordinate(x3, y3, 0);
        coordinates[4] = new Coordinate(x, y,0);
        LinearRing shell = geometryFactory.createLinearRing(coordinates);
        return geometryFactory.createPolygon(shell, null);
    }



    public static Point getPoint(double x, double y) {
        return geometryFactory.createPoint(new Coordinate(x, y));
    }


    public static LineString createLineString(double startX, double startY, double endX, double endY) {
        Coordinate start = new Coordinate(startX, startY);
        Coordinate end = new Coordinate(endX, endY);
        return geometryFactory.createLineString(new Coordinate[]{start, end});
    }




    /**
     * 检测两个路径是否有重叠，并提取重叠的线段
     * @param path1 第一个路径
     * @param path2 第二个路径
     * @return 重叠的线段列表
     */
    public static List<LineString> findOverlappingSegments(LinkedList<RobotMapPosDto> path1, LinkedList<RobotMapPosDto> path2) {
        if (path1 == null || path2 == null || path1.size() < 2 || path2.size() < 2) {
            return Lists.newArrayList();
        }

        GeometryFactory geometryFactory = new GeometryFactory(new PrecisionModel(), 0);

        // 将路径点转换为线段
        List<LineString> lineStrings1 = convertPathToLineStrings(path1, geometryFactory);
        List<LineString> lineStrings2 = convertPathToLineStrings(path2, geometryFactory);

        // 检查转换结果是否为空
        if (lineStrings1.isEmpty() || lineStrings2.isEmpty()) {
            return Lists.newArrayList();
        }

        // 准备几何对象以提高性能
        PreparedGeometryFactory preparedGeometryFactory = new PreparedGeometryFactory();
        List<LineString> overlappingSegments = Lists.newArrayList();

        try {
            for (LineString line1 : lineStrings1) {
                PreparedGeometry preparedLine1 = preparedGeometryFactory.create(line1);
                for (LineString line2 : lineStrings2) {
                    if (preparedLine1.intersects(line2)) {
                        // 计算交集
                        Geometry intersection = line1.intersection(line2);
                        if (!intersection.isEmpty() && intersection instanceof LineString) {
                            overlappingSegments.add((LineString) intersection);
                        }
                    }
                }
            }
        } catch (Exception e) {
            // 记录异常信息
            System.err.println("几何计算过程中发生异常: " + e.getMessage());
            // 可以根据需求选择是否抛出异常或返回空列表
            return Lists.newArrayList();
        }

        return overlappingSegments;
    }

    /**
     * 将路径点列表转换为线段列表
     * @param path 路径点列表
     * @param geometryFactory GeometryFactory实例
     * @return 线段列表
     */
    private static List<LineString> convertPathToLineStrings(LinkedList<RobotMapPosDto> path, GeometryFactory geometryFactory) {
        // 确保路径点的有效性
        if (path == null || path.size() < 2) {
            return Lists.newArrayList();
        }

        List<LineString> lineStrings = Lists.newArrayList();
        for (int i = 0; i < path.size() - 1; i++) {
            RobotMapPosDto point1 = path.get(i);
            RobotMapPosDto point2 = path.get(i + 1);
            // 确保点的有效性
            if (point1 != null && point2 != null) {
                // 转换为线段
                LineString lineString = geometryFactory.createLineString(new org.locationtech.jts.geom.Coordinate[]{
                        new org.locationtech.jts.geom.Coordinate(point1.getPosX(), point1.getPosY()),
                        new org.locationtech.jts.geom.Coordinate(point2.getPosX(), point2.getPosY())
                });
                lineStrings.add(lineString);
            }
        }
        return lineStrings;
    }


    /**
     * 计算路径的方向关系
     * @param path1 第一个路径
     * @param path2 第二个路径
     * @return 方向关系，"同向"、"反向"或"其他"
     */
    public static String calculatePathDirection(LinkedList<RobotMapPosDto> path1, LinkedList<RobotMapPosDto> path2) {
        if (path1 == null || path2 == null || path1.size() < 2 || path2.size() < 2) {
            return "其他";
        }

        // 计算第一个路径的方向向量
        double[] direction1 = calculateDirectionVector(path1);
        // 计算第二个路径的方向向量
        double[] direction2 = calculateDirectionVector(path2);

        // 计算点积
        double dotProduct = direction1[0] * direction2[0] + direction1[1] * direction2[1];
        // 计算模
        double magnitude1 = Math.sqrt(direction1[0] * direction1[0] + direction1[1] * direction1[1]);
        double magnitude2 = Math.sqrt(direction2[0] * direction2[0] + direction2[1] * direction2[1]);

        // 计算夹角的余弦值
        double cosTheta = dotProduct / (magnitude1 * magnitude2);

        if (cosTheta > 0.99) {
            return "同向";
        } else if (cosTheta < -0.99) {
            return "反向";
        } else {
            return "其他";
        }
    }

    /**
     * 计算路径的方向向量
     * @param path 路径点列表
     * @return 方向向量 [x, y]
     */
    private static double[] calculateDirectionVector(LinkedList<RobotMapPosDto> path) {
        RobotMapPosDto start = path.getFirst();
        RobotMapPosDto end = path.getLast();
        return new double[]{end.getPosX() - start.getPosX(), end.getPosY() - start.getPosY()};
    }


    /**
     * 将重叠的线段转换为LinkedList<RobotMapPosDto>集合
     * @param overlappingSegments 重叠的线段列表
     * @return 重叠的路径点集合
     */
    public static LinkedList<RobotMapPosDto> convertOverlappingSegmentsToPath(List<RobotMapPosDto> allMapPos ,List<LineString> overlappingSegments) {
        LinkedList<RobotMapPosDto> overlappingPath = new LinkedList<>();
        for (LineString segment : overlappingSegments) {
            for (Coordinate coord : segment.getCoordinates()) {
                // 判断坐标是否在allMapPos, 如果存在则添加到overlappingPath中
                allMapPos.stream().filter(posDto -> posDto.getPosX() == coord.x && posDto.getPosY() == coord.y).findFirst().ifPresent(overlappingPath::add);
            }
        }
        return overlappingPath;
    }



    /**
     * 计算两个AGV的相对位置
     * @param agv1Pos AGV1的位置坐标
     * @param agv1Direction AGV1的方向向量
     * @param agv2Pos AGV2的位置坐标
     * @return 如果AGV2在AGV1前面返回1，如果AGV1在AGV2前面返回-1，如果两者在同一直线上但方向相反返回0
     */
    /**
     * 计算两个AGV在同向路径上的相对位置
     * @param path 路径点列表
     * @return 如果AGV2在AGV1前面返回1，如果AGV1在AGV2前面返回-1，如果两者在相同位置返回0
     */
    public static int calculateRelativePositionOnSamePath(LinkedList<RobotMapPosDto> path,
                                                          double x1, double y1, double x2, double y2) {

        if (path == null || path.size() < 2) {
            //throw new IllegalArgumentException("路径至少需要两个点");
            return 0;
        }

        Coordinate agv1Pos = new Coordinate(x1, y1);
        Coordinate agv2Pos = new Coordinate(x2, y2);

        // 计算路径的方向向量
        double[] directionVector = calculateDirectionVector(path);
        double directionX = directionVector[0];
        double directionY = directionVector[1];
        // 计算AGV1的位置投影
        double agv1Projection = (agv1Pos.x - path.getFirst().getPosX()) * directionX + (agv1Pos.y - path.getFirst().getPosY()) * directionY;
        // 计算AGV2的位置投影
        double agv2Projection = (agv2Pos.x - path.getFirst().getPosX()) * directionX + (agv2Pos.y - path.getFirst().getPosY()) * directionY;
        // 比较投影值
        if (agv2Projection > agv1Projection) {
            return 1; // AGV2在AGV1前面
        } else if (agv2Projection < agv1Projection) {
            return -1; // AGV1在AGV2前面
        } else {
            return 0; // 两者在相同位置,暂时不可能出现
        }
    }



    public static  double getDistance(Polygon polygon, Point point) {
        return polygon.distance(point);
    }




}
